
from __future__ import print_function



# ==============================================================================
# -- find carla module ---------------------------------------------------------
# ==============================================================================
import collections
import glob
import math
import os
import sys
import time
import weakref
try:
    sys.path.append(glob.glob('C:/carla/PythonAPI/carla/dist/carla-*%d.%d-%s.egg' % (
        sys.version_info.major,
        sys.version_info.minor,
        'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
except IndexError:
    pass
import carla
from datetime import datetime, timedelta
import serial  # 导入模块
import struct
# import matplotlib.pyplot as plt
import time
import pygame
import math
import time
import json
# import _thread
from MQTTConnector import MQTTConnector
from threading import Thread
# Connect to client
client = carla.Client('127.0.0.1', 2000)
client.set_timeout(2.0)

# Get World and Actors
world = client.get_world()
current_map = world.get_map()
actors = world.get_actors()

pygame.joystick.init()
joystick_count = pygame.joystick.get_count()
if joystick_count > 1:
    raise ValueError("Please Connect Just One Joystick")

joystick = pygame.joystick.Joystick(0)
joystick.init()

is_zero = False
is_p = True

def on_getMessage(msgDict,userdata):
    print(str(msgDict))
    global is_zero
    global is_p
    if(msgDict["speed"] == 0):
        is_zero = True
    else:
        is_zero = False
    if(msgDict["gear"] == 'd' or msgDict["gear"] == 'p'):
        is_p = True
    else:
        is_p = False
    
mqttConn = MQTTConnector(
    MQTTConnector.CLIENT_ID_CARLA, MQTTConnector.TASK_TOPIC_VCTL, MQTTConnector.TASK_TOPIC_CARLA, useTLS=True, msg_cb=on_getMessage)
Thread(target=lambda: mqttConn.startConnector("192.168.31.13", 1885), daemon=True).start()

pygame.init()

#获取所有车辆的位置信息
def get_all_location():
    location=[]
    for actor in actors:

        #遍历每一辆车：if 'vehicle' in actor.type_id:

        #遍历每一辆远车
        print(actor.type_id)
        if 'vehicle.audi.tt' == actor.type_id:

            #t = actor.get_transform()
            #v = actor.get_velocity()
            #c = actor.get_control()

            #print(actor.type_id)
            location.append([t.location.x,t.location.y,t.location.z])

    return location


def parse_wheels():
    numAxes = joystick.get_numaxes()
    jsInputs = [float(joystick.get_axis(i)) for i in range(numAxes)]

    # print (jsInputs)
    jsButtons = [float(joystick.get_button(i)) for i in range(joystick.get_numbuttons())]

            # Custom function to map range of inputs [1, -1] to outputs [0, 1] i.e 1 from inputs means nothing is pressed
            # For the steering, it seems fine as it is
    K1 = 1.0  # 0.55
    steerCmd = 2 * K1 * math.tan(1.1 * jsInputs[0])  #方向盘
    if(steerCmd > 3):
        steerCmd = 3

    if(steerCmd < -3):
        steerCmd = -3
    K2 = 1.6  # 1.6
    throttleCmd = K2 + (2.05 * math.log10(
        -0.7 * jsInputs[1] + 1.4) - 1.2) / 0.92
    if throttleCmd <= 0:   #油门
        throttleCmd = 0
    elif throttleCmd > 1:
        throttleCmd = 1

    brakeCmd = 1.6 + (2.05 * math.log10(
        -0.7 * jsInputs[2] + 1.4) - 1.2) / 0.92
    if brakeCmd <= 0:  #刹车
        brakeCmd = 0
    elif brakeCmd > 1:
        brakeCmd = 1
    return - 1 * int(steerCmd * 100), int(20+10*throttleCmd), int(100*brakeCmd)

#获取本车actor（全城都开奥迪，只本车开特斯拉）
def get_my_car():
    for actor in actors:
        if actor.type_id == 'vehicle.charger2020.charger2020':
            return actor
    return 0

#获取车辆速度
def get_speed(actor):
    v = actor.get_velocity()
    speed = int(3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2))  # km/h
    return speed

#获取目标车的gps坐标
def get_gps(actor):
    t = actor.get_transform()
    return xy2gps(t.location.x,t.location.y,t.location.z)



def strconvert(str1):
    str2 = b''
    s = str1.strip().split(' ')
    for i in range(len(s)):
        str2 = str2 + struct.pack('B', int(s[i], 16))
    return str2


def start(str):
    ser.write(str)
    s = ser.read(8).hex()

def position():
    s = ser.read(31).hex()
    x = s[14:18]
    x = int(x, 16)
    x = bin(x)[2:]
    if len(x) < 32:
        x = '0' + x
    if x[0] == '1':
        flag1 = -1
    else:
        flag1 = 1
    x = int(x[1:32], 2) * flag1

    y = s[18:22]
    y = int(y, 16)
    y = bin(y)[2:]
    if len(y) < 32:
        y = '0' + y
    if y[0] == '1':
        flag2 = -1
    else:
        flag2 = 1
    y = int(y[1:32], 2) * flag2
    return x, y


def get_carla_position(map_x=74, map_y=60):
    start(write_msg)
    time.sleep(0.1)
    ser.write(read_msg)
    x, y = position()
    
    sumx = 0
    sumy = 0
    global position_list
    if len(position_list) == 5:
        position_list.append((x,y))
        position_list.pop(0)
        for i in range(5):
            sumx = position_list[i][0] + sumx
            sumy = position_list[i][1] + sumy
        x = sumx / 5
        y = sumy / 5
    else:
        position_list.append((x,y))    

    x = 0.1 * x
    y = 0.1 * y

    return map_x - x, -1 * (map_y - y)

def get_yaw(x_offset, y_offset):
    print(x_offset, y_offset)
    if(x_offset > 4 or y_offset > 4):
        return -1
    OFFSET = 0.1
    if(x_offset < OFFSET and y_offset < OFFSET and x_offset > -1 * OFFSET and y_offset > -1 * OFFSET):
        return -1
    else:
        if(x_offset == 0):
            x_offset = 0.001
        if(y_offset == 0):
            y_offset = 0.001
        yaw = math.atan(y_offset/x_offset) / math.pi * 180
        if(x_offset < 0):
            if(yaw < 0 and yaw > -90):
                yaw += 180
            elif(yaw > 0 and yaw < 90):
                yaw -= 180
    return yaw

        
def g29():
    print("=============================g29====================================")
    steer, throttle, brake = parse_wheels()
    pygame.event.get()
    print("校准控制器...")
    while(throttle != 20 or brake != 0):
        steer, throttle, brake = parse_wheels()
        pygame.event.get()
        if(throttle != 20):
            print("校准油门，throttle : " + str(throttle))
        if(brake != 0):
            print("校准刹车，brake : " + str(brake))
        pygame.time.wait(1000)
    print("-----------------------------------校准成功--------------------------------------")
    print("\n\n\n")
    straight = 'd'
    autoflag = False
    lightflag = False
    leftflag = False
    rightflag = False
    # global is_p
    while True:
        flag = False
        steer, throttle, brake = parse_wheels()
        for event in pygame.event.get():
            if event.type == pygame.JOYBUTTONDOWN:
                if(event.button == 12 and straight == 'd'):
                    flag = True
                    # is_p = False
                    straight = 'r'
                elif(event.button > 12 and event.button <= 17 and straight == 'r'):
                    straight = 'd'
                    # is_p = True
                    flag = True
                if(event.button == 8):
                    flag = True
                    autoflag = not autoflag
                if(event.button == 23):
                    lightflag = not lightflag
                if(event.button == 5):
                    leftflag = not leftflag
                if(event.button == 4):
                    rightflag = not rightflag
                print(event.button)
        light = {"cmd" : "light", "lightflag": lightflag, "leftflag": leftflag, "rightflag": rightflag}
        mqttConn.sendMsgDict(light)
        pygame.time.wait(100)
        if(straight == 'r'):
            throttle += 5
        if(steer < 5 and steer > -5 and throttle == 20 and brake == 0 and not flag and straight == 'd'):
            continue
        if(steer < 5 and steer > -5 and throttle == 25 and brake == 0 and not flag and straight == 'r'):
            continue

        msgDict = {"cmd": "running", "autoflag": autoflag,  "angel": str(steer), "steer_speed": "500", "acc": str(throttle), "gear": straight, "brake" : str(brake)}
        light = {"cmd" : "light", "lightflag": lightflag, "leftflag": leftflag, "rightflag": rightflag}

        mqttConn.sendMsgDict(msgDict)
        

        


def set_car():
    print(" ============================set_car================================")

    global position_list
    position_list = []
    car=get_my_car()

    try:
        

        x_mem, y_mem= get_carla_position()
        location = carla.Location(x_mem,y_mem,20)
        rotation = carla.Rotation(0,0,0)
        transform = carla.Transform(location,rotation)
        car.set_transform(transform)
        
        count = 0
        while True:
            count += 1
            count = count % 10
            x, y= get_carla_position()
            x_offset = x - x_mem
            y_offset = y - y_mem
            yaw = get_yaw(x_offset, y_offset)
            if(not is_p):
                if(yaw < 0):
                    yaw += 180
                else:
                    yaw -= 180
            # if(yaw == -1 or is_zero):
            #     print("zero")
            #     continue


            x_mem = x
            y_mem = y
            location = carla.Location(x,y ,0.2)
            # print(x, y)
            if(count != 0):
                continue
            if(yaw != -1):
                # mov_to(car,x,y)
                rotation = carla.Rotation(0,yaw,0)
                transform = carla.Transform(location,rotation)
                car.set_transform(transform)
            # location = carla.Location(74,-60,20)
            else:
                t = car.get_transform()
                transform = carla.Transform(location,t.rotation)
                car.set_transform(transform)

        
    except Exception as e:
        print("error:", e)


if __name__ == "__main__":


    try:
        portx = "COM3"
        bps = 115200
        # 超时设置,None：永远等待操作，0为立即返回请求结果，其他值为等待超时时间(单位为秒）
        ser = serial.Serial(portx, bps, timeout=None)
        print("串口详情参数：", ser)
        # read_msg = "01 03 00 2A 00 0D A5 C7"
        write_msg = "01 10 00 28 00 01 02 00 01 61 B8"
        # read_msg = "01 10 00 28 00 01 02 00 04 a1 bb"
        read_msg = "01 03 00 2A 00 0D A5 C7"
        write_msg = strconvert(write_msg)
        read_msg = strconvert(read_msg)
        Thread(target=g29).start()
        Thread(target=set_car).start()
    except Exception as e:
        print(str(e))
# ser.close()  # 关闭串口



    